#ifndef _POINTCLOUDFILTERTABLE_H_
#define _POINTCLOUDFILTERTABLE_H_

#include <iostream>
#include <vector>
#include <string>
#include "visgrabTypes.h"
#include "TablePlane.h"

using namespace std;

class PointCloudFilterTable {
 public:
  PointCloudFilterTable(double _sigma = 25.0, double _r1 = 100, double _r2 = 1000, bool horizontalAssumption = false);
  ~PointCloudFilterTable();
  void setParameters(double _sigma, bool _horizontalAssumption);
  void fitTablePlane(PointCloud* pointCloud);
  PointCloud* filter(PointCloud* pointCloudIn);
  double getA() { return planeEquation.a; };
  double getB() { return planeEquation.b; };
  double getC() { return planeEquation.c; };
  double getD() { return planeEquation.d; };
  double getNABC() { return planeEquation.nABC; };

 private:
  bool includeUpward(double x, double y, double z);
  bool includeDownward(double x, double y, double z);

  int minNrRANSACIterations;
  int maxNrRANSACIterations;
  unsigned int minSamples;
  double sigma; // Texlets further from the table are excluded
  double r1;   // All texlets closer are not considered
  double r2;  // All texlets further are not considered
  bool horizontalAssumption;
  PlaneEquation planeEquation;
};

#endif
